REPORT  DOCUMENTATION  PAGE 


Form  Approved  OMB  NO.  0704-0188 


The  public  reporting  burden  for  this  collection  of  information  is  estimated  to  average  1  hour  per  response,  including  the  time  for  reviewing  instructions, 
searching  existing  data  sources,  gathering  and  maintaining  the  data  needed,  and  completing  and  reviewing  the  collection  of  information.  Send  comments 
regarding  this  burden  estimate  or  any  other  aspect  of  this  collection  of  information,  including  suggesstions  for  reducing  this  burden,  to  Washington 
Headquarters  Services,  Directorate  for  Information  Operations  and  Reports,  1215  Jefferson  Davis  Highway,  Suite  1204,  Arlington  VA,  22202-4302. 
Respondents  should  be  aware  that  notwithstanding  any  other  provision  of  law,  no  person  shall  be  subject  to  any  oenalty  for  failing  to  comply  with  a  collection 
of  information  if  it  does  not  display  a  currently  valid  OMB  control  number. 

PLEASE  DO  NOT  RETURN  YOUR  FORM  TO  THE  ABOVE  ADDRESS. 


2.  REPORT  TYPE 
Final  Report 


1.  REPORT  DATE  (DD-MM-YYYY) 

05-03-2016 


4.  TITLE  AND  SUBTITLE  5a.  CONTRACT  NUMBER 

Probabilistic  Tracking  and  Trajectory  Planning  for  Autonomous  W91  INF-09- 1-0466 
Ground  Vehicles  in  Urban  Environments  5b.  GRANT  NUMBER 


3.  DATES  COVERED  (From  -  To) 

26-Aug-2009  -  25-Aug-2014 


6.  AUTHORS 
Mark  E.  Campbell 


5c.  PROGRAM  ELEMENT  NUMBER 
611102 


5d.  PROJECT  NUMBER 


5e.  TASK  NUMBER 


5f.  WORK  UNIT  NUMBER 


7.  PERFORMING  ORGANIZATION  NAMES  AND  ADDRESSES 

Cornell  University 
373  Pine  Tree  Road 


8.  PERFORMING  ORGANIZATION  REPORT 
NUMBER 


Ithaca,  NY  14850  -2820 


9.  SPONSORING/MONITORING  AGENCY  NAME(S)  AND  ADDRESS 
(ES) 

U.S.  Army  Research  Office 
P.O.Box  12211 

Research  Triangle  Park,  NC  27709-2211 


10.  SPONSOR/MONITOR'S  ACRONYM(S) 
ARO 


11.  SPONSOR/MONITOR'S  REPORT 
NUMBER(S) 

56043-NS.36 


12.  DISTRIBUTION  AVAIL1BILITY  STATEMENT 
Approved  for  Public  Release;  Distribution  Unlimited 


13.  SUPPLEMENTARY  NOTES 

The  views,  opinions  and/or  findings  contained  in  this  report  are  those  of  the  author(s)  and  should  not  contrued  as  an  official  Department 
of  the  Army  position,  policy  or  decision,  unless  so  designated  by  other  documentation. 


14.  ABSTRACT 

The  aim  of  this  research  is  to  develop  a  unified  theory  for  perception  and  planning  in  autonomous  ground  vehicles, 
with  a  specific  focus  on  obstacle  tracking/identification  and  trajectory  planning,  so  as  to  enable  reasoned,  intelligent 
planning  rather  than  simple  reactive  planning.  This  final  report  details  our  contributions.  First,  we  developed  a  new 
method  for  anticipating  obstacle  motion  using  Gaussian  Processes  in  order  to  enable  contingency  planning.  Second, 
we  developed  three  new  mapping  strategies  including  a  unified  terrain  model  based  on  a  Markov  Random  Fields; 


_ i 


TU.'„U  A —  ,„1, 


15.  SUBJECT  TERMS 

ground  robots,  autonomous  driving,  probabilistic  anticipation,  trajectory  planning 


16.  SECURITY  CLASSIFICATION  OF: 

17.  LIMITATION  OF 

15.  NUMBER 

a.  REPORT 

b.  ABSTRACT 

c.  THIS  PAGE 

ABSTRACT 

OF  PAGES 

UU 

UU 

UU 

UU 

Mark  Campbell 


19b.  TELEPHONE  NUMBER 
607-255-4268 


Standard  Fonn  298  (Rev  8/98) 
Prescribed  by  ANSI  Std.  Z39.18 


Report  Title 

Probabilistic  Tracking  and  Trajectory  Planning  for  Autonomous  Ground  Vehicles  in  Urban  Environments 

ABSTRACT 

The  aim  of  this  research  is  to  develop  a  unified  theory  for  perception  and  planning  in  autonomous  ground  vehicles,  with  a  specific  focus  on 
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order  to  enable  contingency  planning.  Second,  we  developed  three  new  mapping  strategies  including  a  unified  terrain  model  based  on  a 
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Obstacle  Anticipation  using  Gaussian  Processes 

This  paper  presents  an  adaptive  Gaussian  Mixture  Model  (aGMM)  formulation  for  performing  multiple-step  probabilistic  state 
predictions  using  a  nonparametric  Gaussian  Process  (GP)  regression  model.  The  presented  prediction  algorithm  is  applicable 
to  any  dynamic  system  that  is  challenging  to  model  parametrically,  but  where  data  is  available.  Gaussian  mix-  ture  elements  are 
propagated  through  the  GP  by  analytically  evaluating  expectation  integrals  for  the  moments  of  the  out-  put  distribution.  Two 
metrics  are  presented  and  compared  for  adaptively  splitting  the  initial  state  distribution  into  a  sum  of  Gaussians  to  reduce  the 
effect  of  nonlinearities  on  prediction  accuracy:  (1)  an  analytical  evaluation  of  the  excess  kurto-  sis  which  measures  the  non- 
Gaussianity  of  the  output  distribution,  and  (2)  a  weighted  least-squares  regression  model  which  evaluates  the  local  nonlinearity 
of  the  GP  mapping  with  respect  to  the  input  distribution.  In  addition,  an  on-the-fly  data  selection  method  is  presented  to  reduce 
the  computational  complexity  associated  with  analytically  evaluating  the  higher-order  moments  of  the  GP  output  distribution. 

The  proposed  adaptive  GP-aGMM  formulation  is  applied  to  the  case  of  anticipating  driver  behavior  at  road  intersections  using  a 
GP  driver  behavior  model  in  combination  with  a  parametric  vehicle  model.  Prediction  performance  for  this  scenario  is  evaluated 
using  driving  data  collected  from  three  human  sub- jects  navigating  a  standard  four-way  intersection.  Results  demonstrate  that 
the  presented  prediction  algorithm  is  capable  of  accurately  capturing  multimodal  behavior  in  the  GP  training  data. 

Negative  Information 

A  novel  approach  to  utilize  negative  information  to  improve  the  precision  and  accuracy  of  extended  multiobject  track-  ing  is 
presented.  The  parameterized  probability  density  of  object  tracks  undetected  in  sensor  data  is  updated  via  inferences  about  the 
conditions  necessary  to  result  in  occlusion  of  the  undetected  object.  Negative  information  is  also  leveraged  to  inform  track  ex¬ 
istence  and  data  association,  both  of  which  contribute  to  a  more  sensible  belief  of  the  local  dynamic  scene.  Simulation  and 
experi-  mental  results  are  presented  from  autonomous  driving  scenarios,  demonstrating  that  the  use  of  negative  information 
leads  to  a  more  complete,  accurate,  precise,  and  intuitive  belief  of  the  local  scene,  enabling  high-level  tasks  that  would 
otherwise  be  impractical. 

Contingency  Planning 

This  paper  presents  a  novel  optimization  based  path  planner  capable  of  planning  multiple  contingency  paths  to  directly  account 
for  uncertainties  in  the  future  trajectories  of  dynamic  obstacles.  This  planner  addresses  the  particular  problem  of  probabilistic 
collision  avoidance  for  autonomous  road  vehicles  that  are  required  to  safely  interact,  in  close  proximity,  with  other  vehicles  with 
unknown  intentions.  The  presented  path  planner  utilizes  an  efficient  spline  based  trajectory  representation  and  fast  but  accurate 
collision  probability  bounds  to  simultaneously  optimize  multiple  continuous  contingency  paths  in  real-time.  These  collision 
probability  bounds  are  efficient  enough  for  real-  time  evaluation,  yet  accurate  enough  to  allow  for  practical  close-  proximity 
driving  behaviors  such  as  passing  an  obstacle  vehicle  in  an  adjacent  lane.  An  obstacle  trajectory  clustering  algorithm  is  also 
presented  to  enable  the  path  planner  to  scale  to  multiple-  obstacle  scenarios.  Simulation  results  show  that  the  contingency 
planner  allows  for  a  more  aggressive  driving  style  than  non-  contingency  based  approaches  without  compromising  the  overall 
safety  of  the  robot. 

Probabilistic  Mapping 

A  terrain  mapping  model  is  proposed  using  a  generalized  Markov  Random  Field  (MRF)  representation.  Unlike  previous  work, 
the  proposed  MRF  can  fully  represent  uncer-  tainties  due  to  sensor  pose  and  measurement  errors,  as  well  as  data  association 
errors  in  a  single  model.  Additionally,  neither  homoscedasticity  nor  a  predefined  shape  of  the  likelihood  distri-  bution  is 
assumed.  The  flexibility  of  an  MRF  model  allows  spatial  height  correlations  to  be  incorporated.  The  ability  to  include  spatial 
correlations  not  only  improves  the  accuracy  through  the  benefits  of  Bayesian  prior  modeling,  but  also  serves  as  a  basis  for 
terrain  property  characterization.  Maximum  likelihood  solutions  of  terrain  roughness  are  derived.  Benefits  of  the  proposed  model 
are  demonstrated  experimentally  on  indoor  and  outdoor  datasets.  Results  show  that  the  MRF  model  leads  to  lower  height 
estimation  errors.  In  addition,  the  capability  of  estimating  non-Gaussian  height  distributions  allows  the  information  about 
individual  terrain  features  to  be  preserved.  Finally,  the  model  is  able  to  accurately  estimate  the  roughness  of  the  terrain,  which 
is  beneficial  for  edge  detection  of  obstacles  and  non-traversible  terrain  regions. 

Relative  Mapping 

This  work  develops  a  novel  method  for  autonomous  robotic  navigation  and  mapping  of  large  scale  spaces  with  sparse 
landmarks  and  minimal  sensing.  The  proposed  algorithm  constructs  a  graph-based  map  which  encodes  the  relative  location  of 
landmarks  in  the  environment.  Uncertainty  in  these  locations  is  captured  by  imposing  qualitative  constraints  on  the  relationships 
between  landmarks  observed  by  the  robot.  These  relationships  are  represented  in  terms  of  the  relative  geometrical  layout  of 
landmark  triplets.  A  novel  measurement  method  based  on  camera  imagery  is  presented  which  extends  previous  work  from  the 
field  of  Qualitative  Spatial  Reasoning.  Measurements  are  fused  into  the  map  using  a  deterministic  approach  based  on  iterative 
graph  updates.  Algorithm  performance  is  evaluated  using  a  combination  of  experimentally  collected  vision  data  and  Monte- 
Carlo  simulations. 

Smoothing  for  improved  perception  and  robustness  in  planning 

A  smoothing  method  for  nonlinear  systems  using  Gaussian  mixture  models  is  pre-  sented.  Problems  of  interest  include 
applications  with  highly  nonlinear  dynamics  and/or  measurement  models,  and  sparse  measurements.  Two  new  smoothing 
meth-  ods  are  presented  which  incorporate  adaptive  Gaussian  splitting  in  the  forward  path,  and  condensation  in  the  backward 
path.  A  nonlinear  forward-backward  smoothing  algorithm  with  Gaussian  mixture  models  is  also  enabled  by  a  novel  technique  to 


esti-  mate  the  'backward  corrector1.  An  experiment  of  a  robot  performing  indoor  naviga-  tion  in  a  sparse  featured  environment 
demonstrates  the  performance  of  the  smoothing  algorithms  in  typical  applications.  Experimental  results  demonstrate  that  the 
new  smoothing  algorithms  give  more  accurate  and  consistent  estimates  than  a  traditional  Kalman  smoother,  even  in  the 
presence  of  a  sparse  featured  environment  and  sparse  measurements. 

Fusion  of  stochastic  maps 

The  fusion  of  independently  obtained  stochastic  maps  by  collaborating  mobile  agents  is  considered.  The  proposed  approach 
includes  two  parts:  generalized  likehood  ratio  matching  and  maximum  likelihood  alignment.  In  particular,  an  affine  invariant 
hypergraph  model  is  constructed  for  each  stochastic  map  and  a  bipartite  matching  via  a  linear  program  is  used  to  establish 
landmark  correspondence  between  stochastic  maps.  A  maximum  likelihood  alignment  procedure  is  proposed  to  estimate 
rotation,  translation  and  scale  parameters  in  order  to  construct  a  global  map  of  the  environment.  A  main  feature  of  the  proposed 
approach  is  its  scalability  with  respect  to  the  number  of  landmarks:  the  matching  step  has  polynomial  complexity  and  the 
maximum  likelihood  alignment  solution  is  obtained  in  closed  form.  Experimental  validation  of  the  proposed  fusion  approach  is 
performed  using  the  Victoria  Park  experimental  benchmark. 

Technology  Transfer 

Concepts  and  papers  from  our  work  were  regularly  delivered  to  industry,  particularly  the  automobile  industry  and  the  Army 
Research  Lab,  over  the  years  of  the  project.  This  includes  the  probabilistic  anticipation,  contingency  planning,  and  negative 
information  work.  In  addition,  PhD  students  graduated  and  are  working  for  Tesla,  Nissan,  Bosch,  and  Ford. 


